﻿#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>
#include "GNSSSatPos.h"
#include "ReadN.h"
#include "ReadO.h"
#include "TimeSys.h"
#include "IonosphericCorr.h"
#include "TroposphericCorr.h"
#include "CoordSys.h"
#include "Matrix.h"
#include <tuple>  // 用于make_tuple
/*
* 对于单点定位的理解：
* 单点定位（Single Point Positioning，SPP）是一种基于伪距观测的绝对定位技术，主要有两方面
* 1.	通过广播星历计算得到卫星在某一时刻的三维坐标
* 2.	通过测距码测定的测站-卫星距离（伪距）
* 先根据伪距观测方程得到伪距观测值，其中把伪距方程线性化，然后进行最小二乘结算
* 具体流程如下
* 先读取o文件和n文件，对于卫星导航系统和接收机的概略位置和钟差进行初始化
* 从观测量得到时间、星号、伪距观测值，从星历里得知卫星匹配的星历
* 计算出来卫星在ecef坐标的卫星位置
* 通过误差改正类 接收机种差、电离层对流层误差改正
*最终得到最后的位置
*/

using namespace std;

// 定义光速常量
const double SPEED_OF_LIGHT = 299792458.0;
const double WEEKSEC = 604800.0;

// 计算欧几里得距离
double compute_distance(const XYZ& a, const XYZ& b) {
    return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
}

// 计算卫星钟差改正
double compute_satellite_clock_correction(BroadEphemeris* eph, BaseT obsTime, TimeSys& timeSys) {
    if (!eph) return 0.0;

    // 计算相对于钟差参考时间的时间差
    BaseT toc = timeSys.calend2baseT({
        eph->year, eph->month, eph->day,
        eph->hour, eph->min, static_cast<int>(eph->sec), 0.0
        });

    double dt = timeSys.time_minus(obsTime, toc);

    // 处理周跳的一个程序方法
    if (dt > WEEKSEC / 2.0) {
        dt -= WEEKSEC;
    }
    else if (dt < -WEEKSEC / 2.0) {
        dt += WEEKSEC;
    }

    // 卫星钟差多项式改正
    double clockCorr = eph->clockBias + eph->clockDrift * dt + eph->clockDriftRate * dt * dt;

    return clockCorr * SPEED_OF_LIGHT; // 转换为距离单位(米)
}

// 改进的单点定位函数
XYZ spp_positioning(TimeSys& timeSys, ReadN& readN, ReadO& readO, BDSSatPos& satPosCalc,
    IonosphericCorr& ionoCorr, TroposphericCorr& tropoCorr) {

    cout << "开始单点定位计算..." << endl;

    // 读取观测文件数据
    vector<obsEpoch>* obsData = readO.get_obs();
    if (obsData->empty()) {
        cerr << "观测文件无数据！" << endl;
        return { 0, 0, 0 };
    }

    cout << "观测历元数量: " << obsData->size() << endl;
    cout << "使用第一个历元进行定位: " << obsData->at(0).time << endl;
    cout << "该历元卫星数量: " << obsData->at(0).numSat << endl;

    // 获取电离层参数
    RinexHead& header = readN.get_head();
    double alpha[4], beta[4];
    for (int i = 0; i < 4; i++) {
        alpha[i] = header.ionAlpha[i];
        beta[i] = header.ionBeta[i];
    }
    ionoCorr.set_klb_params(alpha, beta);

    // 初始接收机坐标 (使用地球中心附近的合理猜测值)
    XYZ receiverPos = { -2.0e6, 4.0e6, 4.0e6 }; // 近似中国区域的ECEF坐标

    // SPP 迭代过程
    int maxIterations = 10;
    double tolerance = 1e-4; // 收敛阈值（米）
    double receiverClockBias = 0.0;

    cout << "\n开始迭代求解..." << endl;

    for (int iter = 0; iter < maxIterations; ++iter) {
        vector<XYZ> satPositions;
        vector<double> pseudoRanges;
        vector<double> totalCorrections;
        vector<string> validSats;

        // 遍历当前历元的所有卫星观测
        BaseT obsTime = timeSys.str2baseT(obsData->at(0).time);

        for (const auto& obs : obsData->at(0).listObsSat) {
            string prn = obs.satID;

            // 只处理北斗卫星
            if (prn[0] != 'C') continue;

            // 检查观测值是否有效
            if (obs.values.empty() || obs.values[0] <= 0) {
                continue;
            }

            // 获取星历数据
            BroadEphemeris* eph = readN.getEphemeris(prn, obsTime);
            if (!eph) {
                cout << "警告: 无法获取卫星 " << prn << " 的星历数据" << endl;
                continue;
            }

            // 获取卫星坐标
            XYZ satPos = satPosCalc.get_satPos(prn, obsTime);
            if (satPos.x == 0 && satPos.y == 0 && satPos.z == 0) {
                cout << "警告: 无法计算卫星 " << prn << " 的坐标" << endl;
                continue;
            }

            // 获取伪距观测值（第一个值通常是伪距）
            double pseudoRange = obs.values[0];

            // 计算卫星钟差改正
            double satClockCorr = compute_satellite_clock_correction(eph, obsTime, timeSys);

            // 计算电离层改正
            double ionoCorrection = 0.0;
            try {
                ionoCorrection = ionoCorr.klobuchar_corr(receiverPos, satPos, obsTime, 1575.42e6);
            }
            catch (...) {
                ionoCorrection = 0.0; // 如果计算失败，使用0
            }

            // 计算对流层改正
            double tropoCorrection = 0.0;
            try {
                tropoCorrection = tropoCorr.saastamoinen_corr(receiverPos, satPos);
            }
            catch (...) {
                tropoCorrection = 0.0; // 如果计算失败，使用0
            }

            // 总改正值
            double totalCorrection = satClockCorr + ionoCorrection + tropoCorrection;

            // 保存有效数据
            satPositions.push_back(satPos);
            pseudoRanges.push_back(pseudoRange);
            totalCorrections.push_back(totalCorrection);
            validSats.push_back(prn);
        }

        cout << "迭代 " << iter + 1 << ": 有效卫星数量 = " << satPositions.size() << endl;

        // 至少需要 4 颗卫星进行定位
        if (satPositions.size() < 4) {
            cerr << "错误: 有效卫星数量不足(" << satPositions.size() << " < 4)，无法进行单点定位！" << endl;
            return { 0, 0, 0 };
        }

        // 显示使用的卫星
        cout << "使用卫星: ";
        for (const string& sat : validSats) {
            cout << sat << " ";
        }
        cout << endl;

        // 构造法方程矩阵
        int n = satPositions.size();
        Matrix A(n, 4); // 系数矩阵
        Matrix L(n, 1); // 观测值向量 (残差)

        for (int i = 0; i < n; ++i) {
            // 计算几何距离
            double rho = compute_distance(receiverPos, satPositions[i]);

            // 构造系数矩阵 (方向余弦)
            A[i][0] = -(receiverPos.x - satPositions[i].x) / rho;
            A[i][1] = -(receiverPos.y - satPositions[i].y) / rho;
            A[i][2] = -(receiverPos.z - satPositions[i].z) / rho;
            A[i][3] = 1.0; // 接收机钟差项

            // 计算残差 (观测值 - 计算值)
            double computedRange = rho + receiverClockBias - totalCorrections[i];
            L[i][0] = pseudoRanges[i] - computedRange;
        }

        // 求解法方程: (A^T * A) * dx = A^T * L
        Matrix At = A.transpose();
        Matrix AtA = At * A;
        Matrix AtL = At * L;

      

        Matrix deltaX = AtA * AtL;

        // 更新接收机坐标和钟差
        double dx = deltaX[0][0];
        double dy = deltaX[1][0];
        double dz = deltaX[2][0];
        double dclk = deltaX[3][0];

        receiverPos.x += dx;
        receiverPos.y += dy;
        receiverPos.z += dz;
        receiverClockBias += dclk;

        // 计算改正数的模
        double correction_norm = sqrt(dx * dx + dy * dy + dz * dz);

        cout << "  坐标改正: dx=" << fixed << setprecision(4) << dx
            << "m, dy=" << dy << "m, dz=" << dz << "m" << endl;
        cout << "  钟差改正: " << dclk << "m" << endl;
        cout << "  改正模值: " << correction_norm << "m" << endl;

        // 判断是否收敛
        if (correction_norm < tolerance) {
            cout << "迭代收敛! 总迭代次数: " << iter + 1 << endl;
            break;
        }

        if (iter == maxIterations - 1) {
            cout << "警告: 达到最大迭代次数，可能未完全收敛" << endl;
        }
    }

    cout << "定位计算完成\n" << endl;
    return receiverPos;
}

int main() {
    cout << "=======================================" << endl;
    cout << "    GNSS SPP 单点定位系统测试" << endl;
    cout << "=======================================" << endl << endl;

    // 文件路径
    string navFile = "C:\\1.17N";
    string obsFile = "C:\\1.17O";

    try {
        // 初始化时间系统
        BDSTime bdsTime;
        cout << "初始化北斗时间系统完成" << endl;

        // 初始化文件读取对象
        ReadN readN(navFile);
        ReadO readO(obsFile);
        cout << "初始化文件读取对象完成" << endl;

        // 解析星历文件
        cout << "\n正在解析星历文件: " << navFile << endl;
        if (!readN.parseFile()) {
            cerr << "错误: 星历文件解析失败！" << endl;
            system("pause");
            return -1;
        }
        cout << "星历文件解析成功" << endl;

        // 显示星历基本信息
        readN.print_eph();

        // 解析观测文件
        cout << "\n正在解析观测文件: " << obsFile << endl;
        if (!readO.parse_file()) {
            cerr << "错误: 观测文件解析失败！" << endl;
            system("pause");
            return -1;
        }
        cout << "观测文件解析成功" << endl;

        // 显示观测文件信息
        cout << "\n观测文件头部信息:" << endl;
        readO.print_head();
        cout << "\n前3个历元的观测数据:" << endl;
        readO.print_body(3);

        // 创建北斗卫星位置计算对象
        BDSSatPos bdsSatPos(&bdsTime, &readN);
        cout << "\n卫星位置计算对象创建完成" << endl;

        // 创建电离层改正对象 (使用默认参数，后续会从文件头更新)
        IonosphericCorr ionoCorr;
        cout << "电离层改正对象创建完成" << endl;

        // 创建对流层改正对象 (使用标准大气参数)
        TroposphericCorr tropoCorr(288.15, 1013.25, 50.0); // 15°C, 1013.25mbar, 50%湿度
        cout << "对流层改正对象创建完成" << endl;

        cout << "\n=======================================" << endl;
        cout << "开始执行单点定位..." << endl;
        cout << "=======================================" << endl;

        // 执行单点定位
        XYZ receiverPos = spp_positioning(bdsTime, readN, readO, bdsSatPos, ionoCorr, tropoCorr);

        // 检查定位结果是否有效
        if (receiverPos.x == 0 && receiverPos.y == 0 && receiverPos.z == 0) {
            cerr << "单点定位失败！" << endl;
            system("pause");
            return -1;
        }

        cout << "=======================================" << endl;
        cout << "          定位结果" << endl;
        cout << "=======================================" << endl;

        // 输出ECEF坐标结果
        cout << fixed << setprecision(4);
        cout << "接收机ECEF坐标:" << endl;
        cout << "  X: " << setw(12) << receiverPos.x << " m" << endl;
        cout << "  Y: " << setw(12) << receiverPos.y << " m" << endl;
        cout << "  Z: " << setw(12) << receiverPos.z << " m" << endl;

        // 将结果转换为BLH坐标
        GPSConstant gpsConst;
        WGS84 wgs84(gpsConst.get_refEllip());
        BLH blh = wgs84.XYZ2BLH(receiverPos);

        cout << "\n接收机大地坐标(WGS84):" << endl;
        cout << "  纬度(B): " << setprecision(9) << setw(13) << blh.B << " °" << endl;
        cout << "  经度(L): " << setprecision(9) << setw(13) << blh.L << " °" << endl;
        cout << "  高程(H): " << setprecision(4) << setw(10) << blh.H << " mm" << endl;

        // 转换为度分秒格式显示
        auto deg2dms = [](double deg) {
            int d = static_cast<int>(deg);
            double min_float = (deg - d) * 60.0;
            int m = static_cast<int>(min_float);
            double s = (min_float - m) * 60.0;
            return make_tuple(d, m, s);
            };

        int lat_d = static_cast<int>(blh.B);
        double lat_min = (blh.B - lat_d) * 60;
        int lat_m = static_cast<int>(lat_min);
        double lat_s = (lat_min - lat_m) * 60;
        

        // 经度转换（修复lon_d拼写错误）
        int lon_d = static_cast<int>(blh.L);  // 修正：lon d → lon_d
        double lon_min = (blh.L - lon_d) * 60;
        int lon_m = static_cast<int>(lon_min);
        double lon_s = (lon_min - lon_m) * 60;
        
    }
    catch (const exception& e) {
        cerr << "程序执行过程中发生异常: " << e.what() << endl;
        system("pause");
        return -1;
    }
    catch (...) {
        cerr << "程序执行过程中发生未知错误！" << endl;
        system("pause");
        return -1;
    }

    system("pause");
    return 0;
}